#lang scheme

(provide vector-insert
         ;vector-insert!
         ;vector-insert-right!
         sorted-vector-insert
         sorted-vector-insert-unique
         sorted-vector-insert!
         sorted-vector-insert-right!
         sorted-vector-insert-unique!
         sorted-vector-insert-unique-right!
         )

(require srfi/67
         srfi/71
         ;srfi/2
         "tidbits.ss"
         "test.ss")

; it works also, but can be slower
;(define (sorted-vector-insert! op< v x)
;  (let* ([len (vector-length v)]
;         [pos (binary-search 
;               0 len 
;               (lambda(i) (op< (vector-ref v i) x)))])
;    (if (= pos len) 
;        #f
;        (begin (vector-copy! v (+ 1 pos) v pos (sub1 len))
;               (vector-set! v pos x)
;               #t))))
; returns vector by 1 longer with x inserted at pos 
(define (vector-insert v x pos
                       [start 0] [end (vector-length v)])
  (true! (<= start pos end))
  (build-vector (- end start -1)
                (lambda (i)
                  (let ([i (+ i start)])
                    (if3 (integer-compare i pos)
                         (vector-ref v i)
                         x
                         (vector-ref v (sub1 i)))))))

; returns vector by 1 longer 
(define (sorted-vector-insert op< v x
                              [start 0] [end (vector-length v)]) ; search range
  (true! (<= start end))
  (let ([pos (binary-search 
              start end
              (lambda(i) (op< (vector-ref v i) x)))])
    (vector-insert v x pos start end)))

; if x not a member, returns vector by 1 longer, othervise return v
(define (sorted-vector-insert-unique compare v x
                                     [start 0] [end (vector-length v)])
  (true! (<= start end))
  (let ([exact? pos (binary-search-3 
                     start end
                     (lambda(i) (compare (vector-ref v i) x)))])
    (if exact? ; not unique
        (vector-copy v start end) ; TODO?: do not copy if start=0 && end=(vector-length v)
        (vector-insert v x pos start end))))


(define (vector-insert! v pos x [end (vector-length v)])
  (and (< pos end)
       (begin (vector-copy! v (add1 pos) v pos (sub1 end))
              (vector-set! v pos x)
              #t)))

(define (vector-insert-right! v pos x [start 0])
  (and (<= start pos)
       (begin (vector-copy! v start v (add1 start) (add1 pos))
              (vector-set! v pos x)
              #t)))

; TODO: cmp insted of op< can speed up the binary search
(define (sorted-vector-insert! op< v x 
                               [start 0] [end (vector-length v)] ; mutable range
                               [search-start start] [search-end end]) ; search range
  (true! (< start end))
  (true! (<= search-start search-end))
  (true! (<= start search-start))
  (true! (<= search-end end))
  (let ([pos (binary-search 
              search-start search-end
              (lambda(i) (op< (vector-ref v i) x)))])
    (vector-insert! v pos x end)))


(define (sorted-vector-insert-right! op< v x 
                                     [start 0] [end (vector-length v)] ; mutable range
                                     [search-start start] [search-end end]) ; search range
  (true! (< start end))
  (true! (<= search-start search-end))
  (true! (<= start search-start))
  (true! (<= search-end end))
  (let ([pos (binary-search 
              search-start search-end
              (lambda(i) (op< (vector-ref v i) x)))])
    (vector-insert-right! v (sub1 pos) x start)))


(define (sorted-vector-insert-unique! compare v x
                                      [start 0] [end (vector-length v)] ; mutable range
                                      [search-start start] [search-end end]) ; search range
  (true! (< start end))
  (true! (<= search-start search-end))
  (true! (<= start search-start))
  (true! (<= search-end end))
  (let ([exact? pos (binary-search-3
                     search-start search-end
                     (lambda(i) (compare (vector-ref v i) x)))])
    (and (not exact?)
         (vector-insert! v pos x end))))


(define (sorted-vector-insert-unique-right! compare v x
                                            [start 0] [end (vector-length v)] ; mutable range
                                            [search-start start] [search-end end]) ; search range
  (true! (< start end))
  (true! (< search-start search-end))
  (true! (<= start search-start))
  (true! (<= search-end end))
  (let ([exact? pos (binary-search-3
                     search-start search-end
                     (lambda(i) (compare (vector-ref v i) x)))])
    (and (not exact?)
         (vector-insert-right! v (sub1 pos) x start))))



; +TEST
; TODO: search-range test

; vector-insert
(equal! (vector-insert #(#\H #\e #\l #\o) 33 1 1 3) #(33 #\e #\l))
(equal! (vector-insert #(#\H #\e #\l #\o) 33 2 1 3) #(#\e 33 #\l))
(equal! (vector-insert #(#\H #\e #\l #\o) 33 3 1 3) #(#\e #\l 33))

; vector-insert!
(define (tt>> i)
  (let* ([v (vector 'one 'two 'three 'four)]
        [rc (vector-insert! v i '!)])
    (list rc v)))
(equal! (tt>> 0) '(#t #(!   one two   three)))
(equal! (tt>> 1) '(#t #(one !   two   three)))
(equal! (tt>> 2) '(#t #(one two !     three)))
(equal! (tt>> 3) '(#t #(one two three !    )))
(equal! (tt>> 4) '(#f #(one two three four )))
(equal! (tt>> 5) '(#f #(one two three four )))

; vector-insert-right!
(define (tt<< i)
  (let* ([v (vector 'one 'two 'three 'four)]
         [rc (vector-insert-right! v i '!)])
    (list rc v)))
(equal! (tt<< -1) '(#f #(one two   three four)))
(equal! (tt<<  0) '(#t #(!   two   three four)))
(equal! (tt<<  1) '(#t #(two !     three four)))
(equal! (tt<<  2) '(#t #(two three !     four)))
(equal! (tt<<  3) '(#t #(two three four  !   )))



(define immutable-vector->vector vector-copy)

(define (make-test-vector)
  (immutable-vector->vector #(777 666  0 1 2 3 4  222 111)))

(define (test f x r)
  (let ([v (make-test-vector)])
    (equal! (f < v x 2 7) r)
    ))

(define (test! f x r r2)
  (let ([v (make-test-vector)])
    (eq? (f < v x 2 7) r2)
    (equal! v (vector-append #(777 666) r #(222 111)))))

(test  sorted-vector-insert        -0.5  #(-0.5 0     1     2     3     4    ))
(test  sorted-vector-insert         0    #(    0 0    1     2     3     4    ))
(test  sorted-vector-insert         0.5  #(     0 0.5 1     2     3     4    ))
(test  sorted-vector-insert         1.5  #(     0     1 1.5 2     3     4    ))
(test  sorted-vector-insert         2    #(     0     1    2 2    3     4    ))
(test  sorted-vector-insert         2.5  #(     0     1     2 2.5 3     4    ))
(test  sorted-vector-insert         3.5  #(     0     1     2     3 3.5 4    ))
(test  sorted-vector-insert         4    #(     0     1     2     3    4 4   ))
(test  sorted-vector-insert         4.5  #(     0     1     2     3     4 4.5))

(test! sorted-vector-insert!       -0.5  #(-0.5 0     1     2     3          ) #t)
(test! sorted-vector-insert!        0    #(   0 0     1     2     3          ) #t)
(test! sorted-vector-insert!        0.5  #(     0 0.5 1     2     3          ) #t)
(test! sorted-vector-insert!        1.5  #(     0     1 1.5 2     3          ) #t)
(test! sorted-vector-insert!        2    #(     0     1   2 2     3          ) #t)
(test! sorted-vector-insert!        2.5  #(     0     1     2 2.5 3          ) #t)
(test! sorted-vector-insert!        3.5  #(     0     1     2     3 3.5      ) #t)
(test! sorted-vector-insert!        4    #(     0     1     2     3     4    ) #f)
(test! sorted-vector-insert!        4.5  #(     0     1     2     3     4    ) #f)

(test! sorted-vector-insert-right! -0.5  #(     0     1     2     3     4    ) #f)
(test! sorted-vector-insert-right!  0    #(     0     1     2     3     4    ) #f)
(test! sorted-vector-insert-right!  0.5  #(       0.5 1     2     3     4    ) #t)
(test! sorted-vector-insert-right!  1.5  #(           1 1.5 2     3     4    ) #t)
(test! sorted-vector-insert-right!  2    #(           1     2 2   3     4    ) #t)
(test! sorted-vector-insert-right!  2.5  #(           1     2 2.5 3     4    ) #t)
(test! sorted-vector-insert-right!  3.5  #(           1     2     3 3.5 4    ) #t)
(test! sorted-vector-insert-right!  4    #(           1     2     3     4 4  ) #t)
(test! sorted-vector-insert-right!  4.5  #(           1     2     3     4 4.5) #t)

(define (testu f x r)
  (let ([v (make-test-vector)])
    (equal! (f number-compare v x 2 7) r)
    ))

(define (testu! f x r r2)
  (let ([v (make-test-vector)])
    (eq? (f number-compare v x 2 7) r2)
    (equal! v (vector-append #(777 666) r #(222 111)))))

(testu  sorted-vector-insert-unique         -0.5  #(-0.5 0     1     2     3     4    ))
(testu  sorted-vector-insert-unique          0    #(     0     1     2     3     4    ))
(testu  sorted-vector-insert-unique          0.5  #(     0 0.5 1     2     3     4    ))
(testu  sorted-vector-insert-unique          1.5  #(     0     1 1.5 2     3     4    ))
(testu  sorted-vector-insert-unique          2    #(     0     1     2     3     4    ))
(testu  sorted-vector-insert-unique          2.5  #(     0     1     2 2.5 3     4    ))
(testu  sorted-vector-insert-unique          3.5  #(     0     1     2     3 3.5 4    ))
(testu  sorted-vector-insert-unique          4    #(     0     1     2     3     4    ))
(testu  sorted-vector-insert-unique          4.5  #(     0     1     2     3     4 4.5))

(testu! sorted-vector-insert-unique!        -0.5  #(-0.5 0     1     2     3          ) #t)
(testu! sorted-vector-insert-unique!         0    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique!         0.5  #(     0 0.5 1     2     3          ) #t)
(testu! sorted-vector-insert-unique!         1.5  #(     0     1 1.5 2     3          ) #t)
(testu! sorted-vector-insert-unique!         2    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique!         2.5  #(     0     1     2 2.5 3          ) #t)
(testu! sorted-vector-insert-unique!         3.5  #(     0     1     2     3 3.5      ) #t)
(testu! sorted-vector-insert-unique!         4    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique!         4.5  #(     0     1     2     3     4    ) #f)

(testu! sorted-vector-insert-unique-right!  -0.5  #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique-right!   0    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique-right!   0.5  #(       0.5 1     2     3     4    ) #t)
(testu! sorted-vector-insert-unique-right!   1.5  #(           1 1.5 2     3     4    ) #t)
(testu! sorted-vector-insert-unique-right!   2    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique-right!   2.5  #(           1     2 2.5 3     4    ) #t)
(testu! sorted-vector-insert-unique-right!   3.5  #(           1     2     3 3.5 4    ) #t)
(testu! sorted-vector-insert-unique-right!   4    #(     0     1     2     3     4    ) #f)
(testu! sorted-vector-insert-unique-right!   4.5  #(           1     2     3     4 4.5) #t)

; -TEST